class Boid
{
  private PVector m_position;
  private PVector m_velocity;
  color m_color;
  
  //For synchronous update
  private PVector m_positionNew;
  private PVector m_velocityNew;
  private ArrayList<PVector> m_prevPositions;

  private int m_id;

  private PVector forceMouse(BoidList boidList)
  {
    PVector mouseForce = new PVector(0,0);
    if (mousePressed && mouseButton == LEFT && (boidList.m_worldState.fAttract() || boidList.m_worldState.fRepel()))
    {
      PVector mousePos = new PVector(mouseX, mouseY);
      mouseForce.set(mouseX, mouseY, 0);
      mouseForce.sub(m_position);
      if (boidList.m_worldState.fRepel())
      {
        mouseForce.mult(-1);
      }
      mouseForce.normalize();
      return mouseForce;
    }
    return mouseForce;
  }
  
  private PVector forceCollisionAvoidance(BoidList boidList)
  {
    if (!boidList.m_worldState.fCollision())
      return new PVector(0,0);
    
    PVector retForce = new PVector(0,0);
    float totalWeight = 0;
    
    Map<Boid, QueryResult> neighborhood = boidList.getNeighbors(COLLISION_AVOID_RADIUS, this);
     
    for (Boid neighbor : neighborhood.keySet())
    {
      QueryResult result = neighborhood.get(neighbor);
      float distance = result.distance();
      PVector direction = result.directionFrom();
      direction.mult(-1);
      if (distance == 0)
      {
        distance = EPS;
        direction = new PVector(random(MAX_RANDOM_X) - MAX_RANDOM_X/2, random(MAX_RANDOM_Y) - MAX_RANDOM_Y/2);
      }
      direction.normalize();
      direction.mult(10/(distance*distance));
      totalWeight += (10/(distance*distance));
      float weight = (COLLISION_AVOID_RADIUS - distance);
      direction.mult(weight);
      retForce.add(direction);
    }
    if (totalWeight != 0)
    {
      retForce.div(totalWeight);
    }
    return retForce;
  }
  
  private PVector forceCentering(BoidList boidList)
  {
    PVector retForce = new PVector(0,0);

    if (!boidList.m_worldState.fCentering())
      return retForce;

    float totalWeight = 0;
    Map<Boid, QueryResult> neighborhood = boidList.getNeighbors(CENTERING_RADIUS, this);
     
    for (Boid neighbor : neighborhood.keySet())
    {
      QueryResult result = neighborhood.get(neighbor);
      float distance = result.distance();
      PVector direction = result.directionFrom();
      if (distance == 0)
      {
        distance = EPS;
        direction = new PVector(random(2) - 1, random(2) - 1);
      }
      direction.normalize();
      direction.mult(10/(distance * distance));
      totalWeight += (10/(distance * distance));
      
      float weight = 1 - ((CENTERING_RADIUS - distance)/(CENTERING_RADIUS - COLLISION_AVOID_RADIUS));
      if (weight < 0)
      {
        weight = 0;
      }
      retForce.add(direction);
    }
    if (totalWeight != 0)
    {
      
      retForce.div(totalWeight);
    }
    return retForce;
  }

  private PVector forceVelocityMatch(BoidList boidList)
  {
    PVector retForce = new PVector(0,0);

    if (!boidList.m_worldState.fVelocityMatch())
      return retForce;

    float totalWeight = 0;
    Map<Boid, QueryResult> neighborhood = boidList.getNeighbors(MATCH_RADIUS, this);
     
    for (Boid neighbor : neighborhood.keySet())
    {
      float distance = neighborhood.get(neighbor).distance();
      PVector direction = new PVector(neighbor.velocity().x, neighbor.velocity().y);
      direction.sub(m_velocity);
      if (distance == 0)
      {
        distance = EPS;
        direction = new PVector(random(2) - 1, random(2) - 1);
      }
      direction.normalize();
      direction.mult(10/(distance * distance));
      totalWeight += (10/(distance * distance));
      retForce.add(direction);
    }
    if (totalWeight != 0)
    {
      retForce.div(totalWeight);
    }
    return retForce;
  }
  
  private PVector forceRandom(BoidList boidList)
  {
    if (!boidList.m_worldState.fRandom())
      return new PVector(0,0);

    float randomX = random(2);
    float randomY = random(2);
    PVector forceRandom = new PVector(randomX - 1, randomY - 1);
    return forceRandom;
  }
  
  private void clampVelocity()
  {
    float velMag = m_velocityNew.mag();
    if (velMag > MAX_VELOCITY)
    {
      m_velocityNew.mult(MAX_VELOCITY/velMag);
    }
    if (velMag < MIN_VELOCITY)
    {
      m_velocityNew.mult(MIN_VELOCITY/velMag);
    }
  }
  
  
  public Boid(int id)
  {
    reinit();
    
    m_id = id;
  }
  
  public void reinit()
  {
    float x = random(WIDTH);
    float y = random(HEIGHT);
    m_position = new PVector(x,y);
    float vx = random(1);
    float vy = random(1);
    m_velocity = new PVector(vx, vy);    
    
    m_positionNew = new PVector(m_position.x, m_position.y);
    m_velocityNew = new PVector(m_velocity.x, m_velocity.y);
    int r = (int)random(155) + 100;
    int g = (int)random(155) + 100;
    int b = (int)random(155) + 100;
    m_color = color(r,g,b);
    m_prevPositions = new ArrayList<PVector>();
  }
  
  public void clearPath()
  {
    m_prevPositions.clear();
  }
  
  public void drawTrail()
  {
    int numPrev = m_prevPositions.size();
    int currentAlpha = 50; 
    int increment = (int)(200 / numPrev);
    int i = 0;
    for ( PVector position : m_prevPositions )
    {
      stroke(200,200,200,currentAlpha);
      point(position.x, position.y);
      currentAlpha += increment;
    }
  }
  
  public void draw()
  {
    float x1, x2, x3;
    float y1, y2, y3;
    PVector velocity = new PVector(m_velocity.x, m_velocity.y);
    stroke(m_color);

    velocity.normalize();
    velocity.mult(10);
    x1 = m_position.x + velocity.x;
    y1 = m_position.y + velocity.y;
    x2 = m_position.x - velocity.x;
    y2 = m_position.y - velocity.y;
    x3 = m_position.x - velocity.x;
    y3 = m_position.x - velocity.y;
    ellipse(m_position.x, m_position.y, 2, 2);
    line(m_position.x, m_position.y, m_position.x + velocity.x, m_position.y + velocity.y);
  }

  public void update(BoidList boidList)
  {
    PVector accCollisionAvoid = forceCollisionAvoidance(boidList);
    PVector accCentering = forceCentering(boidList);
    PVector accMouse = forceMouse(boidList);
    PVector accRandom = forceRandom(boidList);
    PVector accFinal = new PVector(0,0);
    PVector accVelocityMatch = forceVelocityMatch(boidList);
    
    //TODO msati3: Potential to use dynamic prioritized acceleration allocation. Static currently.
    boolean accelerationMaxed = false;
    accFinal.add(accVelocityMatch);
    float accFinalMag = accFinal.mag();
    if (accFinalMag > MAX_ACCELERATION)
    {
      accelerationMaxed = true;
      accFinal.sub(accVelocityMatch);
      accVelocityMatch.normalize();
      accVelocityMatch.mult(MAX_ACCELERATION - accFinal.mag());
      accFinal.add(accVelocityMatch);
    }
    if (!accelerationMaxed)
    {
      accFinal.add(accCollisionAvoid);
      accFinalMag = accFinal.mag();
      if (accFinalMag > MAX_ACCELERATION)
      {
        accelerationMaxed = true;
        accFinal.sub(accCollisionAvoid);
        accCollisionAvoid.normalize();
        accCollisionAvoid.mult(MAX_ACCELERATION - accFinal.mag());
        accFinal.add(accCollisionAvoid);
      }
    }
    if (!accelerationMaxed)
    { 
      accFinal.add(accMouse);
      accFinalMag = accFinal.mag();
      if (accFinalMag > MAX_ACCELERATION)
      {
        accelerationMaxed = true;
        accFinal.sub(accMouse);
        accMouse.normalize();
        accMouse.mult(MAX_ACCELERATION - accFinal.mag());
        accFinal.add(accMouse);
      }
    }
    if (!accelerationMaxed)
    {
      accFinal.add(accCentering);
      accFinalMag = accFinal.mag();
      if (accFinalMag > MAX_ACCELERATION)
      {
        accelerationMaxed = true;
        accFinal.sub(accCentering);
        accCentering.normalize();
        accCentering.mult(MAX_ACCELERATION - accFinal.mag());
        accFinal.add(accCentering);
      }
    }
    if (!accelerationMaxed)
    {
       accFinal.add(accRandom);
       accFinalMag = accFinal.mag();
       if (accFinalMag > MAX_ACCELERATION)
       {
         accelerationMaxed = true;
         accFinal.sub(accRandom);
         accRandom.normalize();
         accRandom.mult(MAX_ACCELERATION - accFinal.mag());
         accFinal.add(accRandom);
       }
    }  
    
    accFinal.mult(TIMESTEP);
    m_velocityNew.add(accFinal);
    
    clampVelocity();
    PVector update = new PVector(m_velocityNew.x, m_velocityNew.y);
    update.mult(TIMESTEP);
    m_positionNew.add(update);
    
    //Torroidal wrap
    if (m_positionNew.x > WIDTH)
    {
      m_positionNew.x = (m_positionNew.x - WIDTH);
    }
    if (m_positionNew.y > HEIGHT)
    {
      m_positionNew.y = (m_positionNew.y - HEIGHT);
    }
    if (m_positionNew.x < 0)
    {
      m_positionNew.x = WIDTH + m_positionNew.x;
    }
    if (m_positionNew.y < 0)
    {
      m_positionNew.y = HEIGHT + m_positionNew.y;
    }   
  }
  
  private void applyUpdate()
  {
    m_velocity.set(m_velocityNew.x, m_velocityNew.y, 0);
    storeLastPosition();
    m_position.set(m_positionNew.x, m_positionNew.y, 0);
  }
  
  private void storeLastPosition()
  {
    if (m_prevPositions.size() < MAX_PREV_POSITIONS)
    {
      m_prevPositions.add(new PVector(m_position.x, m_position.y));
    }
    else
    {
      m_prevPositions.remove(0);
      m_prevPositions.add(new PVector(m_position.x, m_position.y));
    }
  }
  
  private PVector velocity()
  {
    return m_velocity;
  }
  
  public PVector position()
  {
    return m_position;
  }
  
  public int id()
  {
    return m_id;
  }
}

